#include "include/h264_dec.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/image_encodings.hpp"
#include "sensor_msgs/msg/image.hpp"

class decoder_node : public rclcpp::Node
{
  private:
	std::string input_url;
	std::string decoder_name;
	std::string scale_size;
	int width, height, size;

	std::shared_ptr<h264_dec> dec;

	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr img_pub;
	std::thread decode_thread;

	void decode_thread_func(void)
	{
		int ret;
		sensor_msgs::msg::Image img;
		img.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
		img.is_bigendian = true;
		img.header.frame_id = "camera";
		auto t = rclcpp::Clock().now();
		RCLCPP_INFO(this->get_logger(), "decode thread running");
		while (rclcpp::ok())
		{
			img.data.resize(this->size);
			ret = this->dec->decode_frame(img.data.data(), size, (int *)&img.width,
										  (int *)&img.height, (int *)&img.step);
			if (ret < 0)
			{
				RCLCPP_WARN(this->get_logger(),
							"error during decode, now try to open the stream again");
				this->dec->close_stream();
				this->dec->open_stream(this->input_url, this->decoder_name, this->scale_size);
				continue;
			}
			RCLCPP_DEBUG(this->get_logger(), "frame decoded: %dx%d, step: %d", img.width,
						 img.height, img.step);
			t = rclcpp::Clock().now();
			img.header.stamp.sec = t.seconds();
			img.header.stamp.nanosec = t.nanoseconds();
			this->img_pub->publish(img);
			RCLCPP_DEBUG(this->get_logger(), "published image");
		}
		RCLCPP_INFO(this->get_logger(), "decode thread exit, rclcpp not ok");
	}

  public:
	decoder_node(std::string name) : Node(name)
	{
		RCLCPP_INFO(this->get_logger(), "node started");
		this->declare_parameter("input_url", "rtsp://admin:root1234@192.168.1.64:554/h264/ch1/main/av_stream");
		this->declare_parameter("decoder_name", "h264_nvv4l2dec");
		this->declare_parameter("scale_size", "640x480");
		this->get_parameter("input_url", this->input_url);
		this->get_parameter("decoder_name", this->decoder_name);
		this->get_parameter("scale_size", this->scale_size);

		RCLCPP_INFO(this->get_logger(), "input_url: %s, decoder_name: %s, scale_size: %s",
					this->input_url.c_str(), this->decoder_name.c_str(),
					this->scale_size.c_str());
		sscanf(this->scale_size.c_str(), "%dx%d", &this->width, &this->height);
		this->size = this->width * this->height * 3;

		this->dec = std::make_shared<h264_dec>();
		if (this->dec->open_stream(this->input_url, this->decoder_name, this->scale_size) < 0)
		{
			RCLCPP_ERROR(this->get_logger(),
						 "failed to open stream with given hardware device");
			return;
		}

		this->img_pub = this->create_publisher<sensor_msgs::msg::Image>("decoded_image", 1);

		this->decode_thread = std::thread(std::bind(&decoder_node::decode_thread_func, this));
		this->decode_thread.detach();

		RCLCPP_INFO(this->get_logger(),
					"decoder initilized, publishing image to topic \"decoded_image\"");
	}

	~decoder_node()
	{
		this->dec->close_stream();
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<decoder_node>("decoder_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
